/*img_
 * learn_faces.cpp
 *
 *  Created on: Mar 23, 2013
 *      Author: lcad
 */
#include <opencv/cv.h>
#include <opencv/highgui.h>
#include "opencv2/core/core.hpp"
#include "opencv2/features2d/features2d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/nonfree/nonfree.hpp"
#include "face_detect.h"
#include "features.h"

using namespace cv;

class Face
{
public:
	IplImage *face_img;
	int image_label;
};

vector<Face> known_faces;

int first_image_sent_to_face_detection = 1;
Mat image_matches;

void
load_knowledge()
{
}


CvCapture*
create_capture()
{
	CvCapture* capture = cvCaptureFromCAM(1);

	if (!capture)
	{
		fprintf(stderr, "ERROR: capture is NULL \n");
		exit(-1);
	}

	return capture;
}


IplImage*
get_new_frame(CvCapture* capture)
{
	IplImage* frame = cvQueryFrame(capture);

	if (!frame)
	{
		fprintf(stderr, "ERROR: frame is null...\n");
		exit(-1);
	}

	return frame;
}


vector<Face>
detect_faces(IplImage *img)
{
	if (first_image_sent_to_face_detection)
	{
		initialize_face_detection();
		first_image_sent_to_face_detection = 0;
	}

	CvSeq* faces = detect_faces_from_image(img);
	vector<Face> faces_vector;

	for (int i = 0; i < (faces ? faces->total : 0); i++)
	{
		Face f;

		CvRect* r = (CvRect*) cvGetSeqElem(faces, i);
		cvSetImageROI(img, *r);

		f.image_label = -1;
		f.face_img = cvCreateImage(cvGetSize(img), img->depth, img->nChannels);
		cvCopyImage(img, f.face_img);

		faces_vector.push_back(f);

		cvResetImageROI(img);
	}

	return faces_vector;
}


void
find_known_faces(IplImage *img, vector<Face> &detected_faces)
{
	int i, j, k;

	SurfFeatureDetector detector(400);
	vector<Mat> descriptors_of_detected_faces, descriptors_of_known_faces;
	vector<KeyPoint> keypoints1, keypoints2;
	SurfDescriptorExtractor extractor;
	Mat descriptors1, descriptors2;

	double known_faces_to_detected_faces_correlation[known_faces.size()][detected_faces.size()];

	for (i = 0; i < known_faces.size(); i++)
		for(j = 0; j < detected_faces.size(); j++)
			known_faces_to_detected_faces_correlation[i][j] = 0.0;

	// detect features from the known faces
	for (i = 0; i < known_faces.size(); i++)
	{
		detector.detect(known_faces[i].face_img, keypoints1);
		extractor.compute(known_faces[i].face_img, keypoints1, descriptors1);
		descriptors_of_known_faces.push_back(descriptors1);
	}

	// detect features from the detected faces
	for (i = 0; i < detected_faces.size(); i++)
	{
		detector.detect(detected_faces[i].face_img, keypoints2);
		extractor.compute(detected_faces[i].face_img, keypoints2, descriptors2);
		descriptors_of_detected_faces.push_back(descriptors2);
	}

	// track the known faces
	for (i = 0; i < known_faces.size(); i++)
	{
		for(j = 0; j < detected_faces.size(); j++)
		{
			// matching descriptors
			BFMatcher matcher(NORM_L2);
			vector<DMatch> matches, good_matches;
			matcher.match(descriptors1, descriptors2, matches);

			double confidence = 0;
			int num_matchings = 0;
			int num_good_matches = 0;

			for (k = 0; k < matches.size(); k++)
			{
				// pode ser uma boa aqui checar se os matchings
				// estao dentro da caixa de um dos rostos

				if(matches[k].distance < 0.25)
				{
					good_matches.push_back(matches[k]);
					num_good_matches ++;
				}
			}

			known_faces_to_detected_faces_correlation[i][j] = (double) num_good_matches / (double) matches.size();
		}
	}

	for(j = 0; j < detected_faces.size(); j++)
	{
		int max_confidence_label;
		double max_confidence = 0;

		for (i = 0; i < known_faces.size(); i++)
		{
			if (known_faces_to_detected_faces_correlation[i][j] > max_confidence)
			{
				max_confidence = known_faces_to_detected_faces_correlation[i][j];
				max_confidence_label = i;
			}
		}

		// aqui eu posso checar se
		// ouve muita duvida entre
		// as outras faces para ter
		// mais precisao
		if (max_confidence > 0.5)
			detected_faces[j].image_label = max_confidence_label;
	}
}


void
add_new_faces(vector<Face> detected_faces)
{
	// colocar essa voz quando nao for alguem conhecido "quem eh você meu chapa? tio filipe te deixou dirigir essa bagaça? henm?"

	// adiciona as faces que nao foram reconhecidas
	for (int i = 0; i < detected_faces.size(); i++)
	{
		if (detected_faces[i].image_label < 0)
		{
			Face new_face;

			IplImage *new_face_img = cvCreateImage(cvGetSize(detected_faces[i].face_img), detected_faces[i].face_img->depth, detected_faces[i].face_img->nChannels);
			cvCopyImage(detected_faces[i].face_img, new_face_img);

			new_face.face_img = new_face_img;
			new_face.image_label = known_faces.size();

			known_faces.push_back(new_face);
		}
	}

	// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
	// !!!! Em algum lugar eu preciso fazer
	// !!!! de condensar a informacao tipo
	// !!!! o merge de neuronios da rede do
	// !!!! patrick... Do jeito que esta alem
	// !!!! de explodir a memoria com o tempo,
	// !!!! a mesma pessoa pode ter varios
	// !!!! labels diferentes.
	// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
}


void
release_detected_faces(vector<Face> detected_faces)
{
	for (int i = 0; i < detected_faces.size(); i++)
		cvReleaseImage(&(detected_faces[i].face_img));
}


void
show_faces_and_labels(vector<Face> faces)
{
	int n = 0;
	char window_name[256];

	for(int i = 0; i < faces.size(); i++)
	{
		if (faces[i].image_label >= 0)
		{
			n++;
			sprintf(window_name, "Face%04d", faces[i].image_label);
			cvShowImage(window_name, faces[i].face_img);
		}
	}

	printf("num faces: %d num learned: %ld\n", n, known_faces.size());
}

IplImage *known_faces_viewer, *known_face_resized;
int num_max_known_faces = 50;
int known_faces_window_height = 500;
int known_faces_window_width = 100;
int known_faces_window_face_width = 20;
int known_faces_window_face_height = 50;
int known_faces_window_num_face_cols = 5;
int known_faces_num_face_rows = 10;
int first_iteraction = 1;

void
show_known_faces()
{
	if (first_iteraction)
	{
		known_faces_viewer = cvCreateImage(cvSize(known_faces_window_width, known_faces_window_height), IPL_DEPTH_8U, 3);
		known_face_resized = cvCreateImage(cvSize(known_faces_window_face_width, known_faces_window_face_height), IPL_DEPTH_8U, 3);
		first_iteraction = 0;
	}

	memset(known_faces_viewer->imageData, 0, known_faces_viewer->imageSize * sizeof(char));

	for(int i = 0; i < known_faces.size(); i++)
	{
		int p_x = i / known_faces_window_num_face_cols;
		int p_y = i % known_faces_num_face_rows;
		int p_start = 3 * (p_y * known_faces_window_width + p_x);

		cvResize(known_faces[i].face_img, known_face_resized, CV_INTER_CUBIC);

		for(int j = 0; j < known_faces_window_face_height; j++)
		{
			for(int k = 0; k < known_faces_window_face_width; k++)
			{
				int p_viewer = p_start + 3 * (j * known_faces_window_face_width + k);
				int p_face = 3 * (j * known_faces_window_face_width + k);

				known_faces_viewer->imageData[p_viewer + 0] = known_face_resized->imageData[p_face + 0];
				known_faces_viewer->imageData[p_viewer + 1] = known_face_resized->imageData[p_face + 1];
				known_faces_viewer->imageData[p_viewer + 2] = known_face_resized->imageData[p_face + 2];
			}
		}
	}

	cvShowImage("known_faces_viewer", known_faces_viewer);
}


int
main(int argc, char **argv)
{
	vector<Face> faces;
	CvCapture* capture = create_capture();

	cvNamedWindow("mywindow", CV_WINDOW_AUTOSIZE);

	load_knowledge();

	while (1)
	{
		IplImage* frame = get_new_frame(capture);
		cvShowImage("mywindow", frame);

		faces = detect_faces(frame);

		if (faces.size() > 0)
		{
			find_known_faces(frame, faces);
			add_new_faces(faces);
			show_faces_and_labels(faces);

			//cvWaitKey(-1);

		}
		//else
		show_known_faces();

		if ((cvWaitKey(10) & 255) == 27) break;
		release_detected_faces(faces);

	}

	cvReleaseCapture(&capture);
	cvDestroyWindow("mywindow");

	return 0;
}




